////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNMemory/inc/WNMemory.h"
#include "WNCore/inc/WNAssert.h"
#include "WNMath/inc/WNMatrix33.h"
#include "WNMath/inc/WNMatrix44.h"
#include "WNMath/inc/WNVector2.h"
#include "WNMath/inc/WNVector3.h"

namespace WNMath {
    WNMatrix33 operator * (WN_FLOAT32 _f, const WNMatrix33& _matrix) {
        return(WNMatrix33(_f * _matrix.m11, _f * _matrix.m12, _f * _matrix.m13,
                          _f * _matrix.m21, _f * _matrix.m22, _f * _matrix.m23,
                          _f * _matrix.m31, _f * _matrix.m32, _f * _matrix.m33));
    }

    WNMatrix33 operator / (WN_FLOAT32 _f, const WNMatrix33& _matrix) {
        const WN_FLOAT32 fInv = 1.0f / _f;

        return(WNMatrix33(fInv * _matrix.m11, fInv * _matrix.m12, fInv * _matrix.m13,
                          fInv * _matrix.m21, fInv * _matrix.m22, fInv * _matrix.m23,
                          fInv * _matrix.m31, fInv * _matrix.m32, fInv * _matrix.m33));
    }
}

using namespace WNMath;
using namespace WNMemory;

// Constructors ////////////////////////////////////////////////////////////////////////////////////

WNMatrix33::WNMatrix33() :
    m11(1.0f), m12(0.0f), m13(0.0f),
    m21(0.0f), m22(1.0f), m23(0.0f),
    m31(0.0f), m32(0.0f), m33(1.0f) {
}

WNMatrix33::WNMatrix33(const WN_FLOAT32* _f) :
    m11(_f[0]), m12(_f[1]), m13(_f[2]),
    m21(_f[3]), m22(_f[4]), m23(_f[5]),
    m31(_f[6]), m32(_f[7]), m33(_f[8]) {
}

WNMatrix33::WNMatrix33(const WNVector2& _vector1,
                       const WNVector2& _vector2,
                       const WNVector2& _vector3) :
    m11(_vector1.x), m12(_vector2.x), m13(_vector3.x),
    m21(_vector1.y), m22(_vector2.y), m23(_vector3.y),
          m31(0.0f),       m32(0.0f),       m33(1.0f) {
}

WNMatrix33::WNMatrix33(WN_FLOAT32 _m11, WN_FLOAT32 _m12, WN_FLOAT32 _m13,
                       WN_FLOAT32 _m21, WN_FLOAT32 _m22, WN_FLOAT32 _m23,
                       WN_FLOAT32 _m31, WN_FLOAT32 _m32, WN_FLOAT32 _m33) :
    m11(_m11), m12(_m12), m13(_m13),
    m21(_m21), m22(_m22), m23(_m23),
    m31(_m31), m32(_m32), m33(_m33) {
}

// Operators ///////////////////////////////////////////////////////////////////////////////////////

WN_FLOAT32& WNMatrix33::operator () (WN_INT32 _nRow, WN_INT32 _nCol) {
    WN_DEBUG_ASSERT(_nRow < 3 && _nCol < 3);

    return(*(&m11 + (((_nRow - 1) * 3) + _nCol)));
}

WN_FLOAT32 WNMatrix33::operator () (WN_INT32 _nRow, WN_INT32 _nCol) const {
    WN_DEBUG_ASSERT(_nRow < 3 && _nCol < 3);

    return(*(&m11 + (((_nRow - 1) * 3) + _nCol)));
}

WNMatrix33::operator WN_FLOAT32* () {
    return(&m11);
}

WNMatrix33::operator const WN_FLOAT32* () const {
    return(&m11);
}

WNMatrix33& WNMatrix33::operator *= (const WNMatrix33& _matrix) {
    const WNMatrix33 mat = *this;

    m11 = mat.m11 * _matrix.m11 + mat.m12 * _matrix.m21 + mat.m13 * _matrix.m31;
    m12 = mat.m11 * _matrix.m12 + mat.m12 * _matrix.m22 + mat.m13 * _matrix.m32;
    m13 = mat.m11 * _matrix.m13 + mat.m12 * _matrix.m23 + mat.m13 * _matrix.m33;
    m21 = mat.m21 * _matrix.m11 + mat.m22 * _matrix.m21 + mat.m23 * _matrix.m31;
    m22 = mat.m21 * _matrix.m12 + mat.m22 * _matrix.m22 + mat.m23 * _matrix.m32;
    m23 = mat.m21 * _matrix.m13 + mat.m22 * _matrix.m23 + mat.m23 * _matrix.m33;
    m31 = mat.m31 * _matrix.m11 + mat.m32 * _matrix.m21 + mat.m33 * _matrix.m31;
    m32 = mat.m31 * _matrix.m12 + mat.m32 * _matrix.m22 + mat.m33 * _matrix.m32;
    m33 = mat.m31 * _matrix.m13 + mat.m32 * _matrix.m23 + mat.m33 * _matrix.m33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator += (const WNMatrix33& _matrix) {
    m11 += _matrix.m11; m12 += _matrix.m12; m13 += _matrix.m13;
    m21 += _matrix.m21; m22 += _matrix.m22; m23 += _matrix.m23;
    m31 += _matrix.m31; m32 += _matrix.m32; m33 += _matrix.m33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator -= (const WNMatrix33& _matrix) {
    m11 -= _matrix.m11; m12 -= _matrix.m12; m13 -= _matrix.m13;
    m21 -= _matrix.m21; m22 -= _matrix.m22; m23 -= _matrix.m23;
    m31 -= _matrix.m31; m32 -= _matrix.m32; m33 -= _matrix.m33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator *= (WN_FLOAT32 _f) {
    m11 *= _f; m12 *= _f; m13 *= _f;
    m21 *= _f; m22 *= _f; m23 *= _f;
    m31 *= _f; m32 *= _f; m33 *= _f;

    return(*this);
}

WNMatrix33& WNMatrix33::operator /= (WN_FLOAT32 _f) {
    const WN_FLOAT32 fInv = 1.0f / _f;

    m11 *= fInv; m12 *= fInv; m13 *= fInv;
    m21 *= fInv; m22 *= fInv; m23 *= fInv;
    m31 *= fInv; m32 *= fInv; m33 *= fInv;

    return(*this);
}

WNMatrix33 WNMatrix33::operator + () const {
    return(*this);
}

WNMatrix33 WNMatrix33::operator - () const {
    return(WNMatrix33(-m11, -m12, -m13,
                      -m21, -m22, -m23,
                      -m31, -m32, -m33));
}

WNMatrix33 WNMatrix33::operator * (const WNMatrix33& _matrix) const {
    const WNMatrix33 mat1 = *this;
    const WNMatrix33 mat2 = _matrix;

    WNMatrix33 mat;

    mat.m11 = mat1.m11 * mat2.m11 + mat1.m12 * mat2.m21 + mat1.m13 * mat2.m31;
    mat.m12 = mat1.m11 * mat2.m12 + mat1.m12 * mat2.m22 + mat1.m13 * mat2.m32;
    mat.m13 = mat1.m11 * mat2.m13 + mat1.m12 * mat2.m23 + mat1.m13 * mat2.m33;
    mat.m21 = mat1.m21 * mat2.m11 + mat1.m22 * mat2.m21 + mat1.m23 * mat2.m31;
    mat.m22 = mat1.m21 * mat2.m12 + mat1.m22 * mat2.m22 + mat1.m23 * mat2.m32;
    mat.m23 = mat1.m21 * mat2.m13 + mat1.m22 * mat2.m23 + mat1.m23 * mat2.m33;
    mat.m31 = mat1.m31 * mat2.m11 + mat1.m32 * mat2.m21 + mat1.m33 * mat2.m31;
    mat.m32 = mat1.m31 * mat2.m12 + mat1.m32 * mat2.m22 + mat1.m33 * mat2.m32;
    mat.m33 = mat1.m31 * mat2.m13 + mat1.m32 * mat2.m23 + mat1.m33 * mat2.m33;

    return(mat);
}

WNMatrix33 WNMatrix33::operator + (const WNMatrix33& _matrix) const {
    return(WNMatrix33(m11 + _matrix.m11, m12 + _matrix.m12, m13 + _matrix.m13,
                      m21 + _matrix.m21, m22 + _matrix.m22, m23 + _matrix.m23,
                      m31 + _matrix.m31, m32 + _matrix.m32, m33 + _matrix.m33));
}

WNMatrix33 WNMatrix33::operator - (const WNMatrix33& _matrix) const {
    return(WNMatrix33(m11 - _matrix.m11, m12 - _matrix.m12, m13 - _matrix.m13,
                      m21 - _matrix.m21, m22 - _matrix.m22, m23 - _matrix.m23,
                      m31 - _matrix.m31, m32 - _matrix.m32, m33 - _matrix.m33));
}

WNMatrix33 WNMatrix33::operator * (WN_FLOAT32 _f) const {
    return(WNMatrix33(m11 * _f, m12 * _f, m13 * _f,
                      m21 * _f, m22 * _f, m23 * _f,
                      m31 * _f, m32 * _f, m33 * _f));
}

WNMatrix33 WNMatrix33::operator / (WN_FLOAT32 _f) const {
    const WN_FLOAT32 fInv = 1.0f / _f;

    return(WNMatrix33(m11 * fInv, m12 * fInv, m13 * fInv,
                      m21 * fInv, m22 * fInv, m23 * fInv,
                      m31 * fInv, m32 * fInv, m33 * fInv));
}

WN_BOOL WNMatrix33::operator == (const WNMatrix33& _matrix) const {
    return(0 == WNMemCmp(this, &_matrix, sizeof(WNMatrix33)));
}

WN_BOOL WNMatrix33::operator != (const WNMatrix33& _matrix) const {
    return(0 != WNMemCmp(this, &_matrix, sizeof(WNMatrix33)));
}

// Members /////////////////////////////////////////////////////////////////////////////////////////

WN_VOID WNMatrix33::Zero() {
    WNMemSet(&m11, 0, sizeof(WNMatrix33));
}

WN_VOID WNMatrix33::Set(const WN_FLOAT32* _f) {
    WNMemCpy(&m11, _f, sizeof(WNMatrix33));
}

WN_VOID WNMatrix33::Set(WN_FLOAT32 _m11, WN_FLOAT32 _m12, WN_FLOAT32 _m13,
                        WN_FLOAT32 _m21, WN_FLOAT32 _m22, WN_FLOAT32 _m23,
                        WN_FLOAT32 _m31, WN_FLOAT32 _m32, WN_FLOAT32 _m33) {
    m11 = _m11; m12 = _m12; m13 = _m13;
    m21 = _m21; m22 = _m22; m23 = _m23;
    m31 = _m31; m32 = _m32; m33 = _m33;
}

WN_VOID WNMatrix33::Identity() {
    m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
    m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
    m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
}

WN_BOOL WNMatrix33::IsIdentity() const {
    return(m11 == 1.0f && m12 == 0.0f && m13 == 0.0f &&
           m21 == 0.0f && m22 == 1.0f && m23 == 0.0f &&
           m31 == 0.0f && m32 == 0.0f && m33 == 1.0f);
}

WN_BOOL WNMatrix33::IsSymmetric() const {
    if ((m12 - m21) != 0.0f) {
        return(WN_FALSE);
    }

    if ((m13 - m31) != 0.0f) {
        return(WN_FALSE);
    }

    if ((m23 - m32) != 0.0f) {
        return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_BOOL WNMatrix33::IsDiagonal() const {
    if (m12 != 0.0f || m13 != 0.0f ||
        m21 != 0.0f || m23 != 0.0f ||
        m31 != 0.0f || m32 != 0.0f) {
        return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_BOOL WNMatrix33::IsRotated() const {
    if (m12 == 0.0f && m21 == 0.0f) {
            return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_FLOAT32 WNMatrix33::Determinant() const {
    const WNMatrix33 mat = *this;

    return((mat.m11 * mat.m22 * mat.m33 + mat.m12 * mat.m23 * mat.m31 + mat.m13 * mat.m21 * mat.m32) -
           (mat.m31 * mat.m22 * mat.m13 + mat.m32 * mat.m23 * mat.m11 + mat.m33 * mat.m21 * mat.m12));
}

WN_FLOAT32 WNMatrix33::Trace() const {
    const WNMatrix33 mat = *this;

    return(mat.m11 + mat.m22 + mat.m33);
}

WN_VOID WNMatrix33::Transpose() {
    const WNMatrix33 mat = *this;

    m11 = mat.m11;
    m12 = mat.m21;
    m13 = mat.m31;
    m21 = mat.m12;
    m22 = mat.m22;
    m23 = mat.m32;
    m31 = mat.m13;
    m32 = mat.m23;
    m33 = mat.m33;
}

WN_BOOL WNMatrix33::Invert() {
    const WN_FLOAT32 determinant = Determinant();

    if (determinant == 0.0f) {
        return(WN_FALSE);
    }

    const WN_FLOAT32 invDet = 1.0f / determinant;
    const WNMatrix33 adjoint = GetAdjugate();

    *this = adjoint * invDet;

    return(WN_TRUE);
}

WN_VOID WNMatrix33::SetRow(WN_UINT8 _row, const WNVector3& _data) {
    WN_DEBUG_ASSERT(_row > 2);

    const WN_UINT8 offset = _row * 3;

    *(&m11 + offset) = _data.x;
    *(&m11 + offset + 1) = _data.y;
    *(&m11 + offset + 2) = _data.z;
}

WN_VOID WNMatrix33::SetColumn(WN_UINT8 _column, const WNVector3& _data) {
    WN_DEBUG_ASSERT(_column > 2);

    *(&m11 + _column) = _data.x;
    *(&m11 + _column + 3) = _data.y;
    *(&m11 + _column + 6) = _data.z;
}

WNMatrix33 WNMatrix33::GetTransposed() const {
    WNMatrix33 mat = *this;

    return(mat.Transpose(), mat);
}

WNMatrix33 WNMatrix33::GetInverted() const {
    WNMatrix33 mat = *this;

    return(mat.Invert(), mat);
}

WNMatrix33 WNMatrix33::GetAdjugate() const {
    WNMatrix33 matrix = *this;

    const WN_FLOAT32 mat11 = matrix.m22 * matrix.m33 - matrix.m23 * matrix.m32;
    const WN_FLOAT32 mat12 = -(matrix.m12 * matrix.m33 - matrix.m13 * matrix.m32);
    const WN_FLOAT32 mat13 = matrix.m12 * matrix.m23 - matrix.m13 * matrix.m22;
    const WN_FLOAT32 mat21 = -(matrix.m21 * matrix.m33 - matrix.m23 * matrix.m31);
    const WN_FLOAT32 mat22 = matrix.m11 * matrix.m33 - matrix.m13 * matrix.m31;
    const WN_FLOAT32 mat23 = -(matrix.m11 * matrix.m23 - matrix.m13 * matrix.m21);
    const WN_FLOAT32 mat31 = matrix.m21 * matrix.m32 - matrix.m22 * matrix.m31;
    const WN_FLOAT32 mat32 = -(matrix.m11 * matrix.m32 - matrix.m12 * matrix.m31);
    const WN_FLOAT32 mat33 = matrix.m11 * matrix.m22 - matrix.m12 * matrix.m21;

    matrix.m11 = mat11;
    matrix.m12 = mat12;
    matrix.m13 = mat13;
    matrix.m21 = mat21;
    matrix.m22 = mat22;
    matrix.m23 = mat23;
    matrix.m31 = mat31;
    matrix.m32 = mat32;
    matrix.m33 = mat33;

    return(matrix);
}

WNVector3 WNMatrix33::GetRow(WN_UINT8 _row) const {
    WN_DEBUG_ASSERT(_row > 2);

    const WN_UINT8 offset = _row * 3;

    return(WNVector3(*(&m11 + offset),
                       *(&m11 + offset + 1),
                       *(&m11 + offset + 2)));
}

WNVector3 WNMatrix33::GetColumn(WN_UINT8 _column) const {
    WN_DEBUG_ASSERT(_column > 2);

    return(WNVector3(*(&m11 + _column),
                     *(&m11 + _column + 3),
                     *(&m11 + _column + 6)));
}